#include <rclcpp/rclcpp.hpp>
#include <image_transport/image_transport.hpp>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>  
#include <opencv2/calib3d/calib3d.hpp> 
#include <rclcpp_components/register_node_macro.hpp>
#include <vector>

namespace palomino {

class pnpsolve : public rclcpp::Node {
public:
    pnpsolve(const rclcpp::NodeOptions& options) 
        : Node("pnpsolve_node", rclcpp::NodeOptions(options).use_intra_process_comms(true)) {
        sub_ = this->create_subscription<sensor_msgs::msg::Image>(
            "image_raw",
            1,
            std::bind(&pnpsolve::imageCallback, this, std::placeholders::_1)
        );

        cv::namedWindow("view");
        cv::startWindowThread();
        RCLCPP_INFO(this->get_logger(), "pnpsolve node initialized and subscribed to image_raw");
    }

    ~pnpsolve() {
        cv::destroyWindow("view");
        RCLCPP_INFO(this->get_logger(), "pnpsolve node destroyed");
    }
    
 
    std::vector<cv::Point> myApprox(const std::vector<cv::Point>& con) {
        double num = 0.001;  // 初始比例系数
        double perimeter = cv::arcLength(con, true);
        double ep = num * perimeter;
        std::vector<cv::Point> approx;
        
        cv::approxPolyDP(con, approx, ep, true);
        
        // 迭代调整epsilon，直到轮廓顶点数<=4
        while (approx.size() > 4) {
            num *= 1.5;
            ep = num * perimeter;
            cv::approxPolyDP(con, approx, ep, true);
        }
        
        return approx;
    }


    cv::Mat preprocessWithQuadApprox(const cv::Mat& input, std::vector<cv::Point>& quad) {
        // 提取紫色区域掩码
        cv::Mat hsv, mask;
        cv::Scalar lower_purple(120, 50, 50);
        cv::Scalar upper_purple(150, 255, 255);
        cv::cvtColor(input, hsv, cv::COLOR_BGR2HSV);
        cv::inRange(hsv, lower_purple, upper_purple, mask);
        
        // 形态学操作
        cv::Mat kernel = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(5, 5));
        cv::morphologyEx(mask, mask, cv::MORPH_CLOSE, kernel);
        cv::morphologyEx(mask, mask, cv::MORPH_OPEN, kernel);
        
        // 查找轮廓
        std::vector<std::vector<cv::Point>> contours;
        cv::findContours(mask, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE);
        
        // 筛选最大轮廓
        if (contours.empty()) {
            quad.clear();
            return input.clone();
        }
        int max_idx = 0;
        double max_area = cv::contourArea(contours[0]);
        for (size_t i = 1; i < contours.size(); ++i) {
            double area = cv::contourArea(contours[i]);
            if (area > max_area) {
                max_area = area;
                max_idx = i;
            }
        }
        std::vector<cv::Point> max_contour = contours[max_idx];
        
        // 四边形逼近
        quad = myApprox(max_contour);
        
        // 绘制四边形
        cv::Mat result = input.clone();
        if (!quad.empty()) {
            cv::polylines(result, quad, true, cv::Scalar(0, 255, 0), 2);
        }
        RCLCPP_INFO(this->get_logger(), "%d %d",result.rows,result.cols);
        return result;
    }

    std::vector<cv::Point3d> boardPoints=std::vector<cv::Point3d>{
        cv::Point3d(-50, -30, 0),
        cv::Point3d(-50, 30, 0),
        cv::Point3d(50, 30, 0),
        cv::Point3d(50, -30, 0)
    };
    std::vector<cv::Point> quad_points;
    std::vector<cv::Point2d> quadPoints;

    // cv::Mat camera1Matrix = (cv::Mat_<double>(3, 3) << 
    //         1332.430339405382,  0.0,  615.2969967506161,
    //         0.0,  132.9264612227262,  339.5233927946428,
    //         0.0,  0.0,  1.0);

    cv::Mat cameraMatrix = (cv::Mat_<double>(3, 3) << 
            757.5764,0.0,        639.8048,
            0.0,     758.0728,   362.9302,
            0.0,     0.0,        1.0);

    // 畸变系数（k1, k2, p1, p2, k3）
    // cv::Mat distortionCoefficients1 = (cv::Mat_<double>(5, 1) <<
    // -0.1755  , -0.1459 ,0 ,0, 0);

    cv::Mat distortionCoefficients = (cv::Mat_<double>(5, 1) <<
    0.1198 ,  -0.1706 ,0 ,0 ,0);

    cv::Mat rvec;
    cv::Mat tvec;

    bool solvePnP(const std::vector<cv::Point3d>& objectPoints,
                  const std::vector<cv::Point2d>& imagePoints,
                  cv::Mat& rvec, cv::Mat& tvec) {
        if (objectPoints.size() != imagePoints.size() ||objectPoints.empty()) {
            RCLCPP_ERROR(this->get_logger(), "3D点和2D点数量不匹配或为空");
            return false;
        }
        // RCLCPP_INFO(this->get_logger(),"%f %f", imagePoints.at(0).x, imagePoints.at(0).y);
        // RCLCPP_INFO(this->get_logger(),"%f %f", imagePoints.at(1).x, imagePoints.at(1).y);
        // RCLCPP_INFO(this->get_logger(),"%f %f", imagePoints.at(2).x, imagePoints.at(2).y);
        // RCLCPP_INFO(this->get_logger(),"%f %f\n", imagePoints.at(3).x, imagePoints.at(3).y);

        // 使用迭代法解PnP
        bool success = cv::solvePnP(
            objectPoints, 
            imagePoints, 
            cameraMatrix, 
            distortionCoefficients, 
            rvec, 
            tvec, 
            false, 
            cv::SOLVEPNP_AP3P
        );

        if (!success) {
            RCLCPP_ERROR(this->get_logger(), "PnP解算失败");
        }
        return success;
    }

    bool reorderQuadPoints(std::vector<cv::Point>& quad_points) {
    if (quad_points.size() != 4) {
        return false;
    }

    std::vector<double> edge_lengths(4);
    for (int i = 0; i < 4; ++i) {
        int j = (i + 1) % 4;
        edge_lengths[i] = cv::norm(quad_points[i] - quad_points[j]);
    }

    std::vector<std::pair<int, double>> edge_index_len;
    for (int i = 0; i < 4; ++i) {
        edge_index_len.emplace_back(i, edge_lengths[i]);
    }

    std::sort(edge_index_len.begin(), edge_index_len.end(),
              [](const std::pair<int, double>& a, const std::pair<int, double>& b) {
                  return a.second > b.second;
              });

    int edge1_idx = edge_index_len[0].first;
    int edge2_idx = edge_index_len[1].first;

    auto getEdgeYMean = [&](int edge_idx) {
        int p1 = edge_idx;
        int p2 = (edge_idx + 1) % 4;
        return (quad_points[p1].y + quad_points[p2].y) / 2.0;
    };

    double y_mean1 = getEdgeYMean(edge1_idx);
    double y_mean2 = getEdgeYMean(edge2_idx);

    int target_edge_idx = (y_mean1 <= y_mean2) ? edge1_idx : edge2_idx;

    int right_end_idx = (target_edge_idx + 1) % 4;

    std::vector<cv::Point> adjusted_quad(4);
    for (int i = 0; i < 4; ++i) {
        adjusted_quad[i] = quad_points[(right_end_idx + i) % 4];
    }

    quad_points = adjusted_quad;
    return true;
}

private:
    void imageCallback(std::unique_ptr<sensor_msgs::msg::Image> msg) {  
        RCLCPP_DEBUG(this->get_logger(), "通信后内存地址为%p", static_cast<void*>(msg.get()));
        RCLCPP_DEBUG(this->get_logger(), "picture type: %s", msg->encoding.c_str());

        cv::Mat image(msg->height, msg->width, CV_8UC3, msg->data.data());
        cv::cvtColor(image, image, cv::COLOR_RGB2BGR);

        cv::Mat processedImage = preprocessWithQuadApprox(image, quad_points);
        if (quad_points.size() == 4){
            reorderQuadPoints(quad_points);
        }

        for (size_t i = 0; i < quad_points.size(); ++i) {
            // 绘制红色圆圈标记点
            cv::circle(processedImage, quad_points[i], 5, cv::Scalar(0, 0, 255), 2);
            // 标注序号
            std::string text = std::to_string(i); // 序号：0、1、2、3
            cv::putText(processedImage, text, 
                        quad_points[i] + cv::Point(10, 0),
                        cv::FONT_HERSHEY_SIMPLEX, 1.0,
                        cv::Scalar(255, 255, 255), 2);
            // 打印坐标到终端
            // RCLCPP_INFO(this->get_logger(), "点 %zu 坐标: (x: %d, y: %d)", 
            //             i, quad_points[i].x, quad_points[i].y);
            // }
            
            cv::Mat mat_points(quad_points);
            if (quad_points.size() == 4) {
                quadPoints.clear();
                for (const auto& pt : quad_points) {
                    quadPoints.emplace_back(static_cast<double>(pt.x), static_cast<double>(pt.y));
                }
            }
        // RCLCPP_INFO(this->get_logger(), "3D点数量为：%zu", boardPoints.size());
        // RCLCPP_INFO(this->get_logger(), "2D点数量为：%zu", quadPoints.size());
        bool pnp_success = solvePnP(boardPoints, quadPoints, rvec, tvec);

        if (pnp_success) {
            RCLCPP_INFO(this->get_logger(), "旋转向量 (rvec):");
            for (int i = 0; i < rvec.rows; ++i) {
                for (int j = 0; j < rvec.cols; ++j) {
                    RCLCPP_INFO(this->get_logger(), "rvec[%d][%d] = %.6f", i, j, rvec.at<double>(i, j));
                }
            }
            
            RCLCPP_INFO(this->get_logger(), "平移向量 (tvec):");
            for (int i = 0; i < tvec.rows; ++i) {
                for (int j = 0; j < tvec.cols; ++j) {
                    RCLCPP_INFO(this->get_logger(), "tvec[%d][%d] = %.6f", i, j, tvec.at<double>(i, j));
                }
            }

        double distance = sqrt(tvec.at<double>(1,0)*tvec.at<double>(1,0)
        +tvec.at<double>(2,0)*tvec.at<double>(2,0)
        +tvec.at<double>(0,0)*tvec.at<double>(0,0));
        RCLCPP_INFO(this->get_logger(), "distance:%.6f", distance);
        }

        // 显示图像
        cv::imshow("view", processedImage);
        cv::waitKey(3);
    }
}
    std::shared_ptr<rclcpp::Subscription<sensor_msgs::msg::Image>> sub_;

};
}
RCLCPP_COMPONENTS_REGISTER_NODE(palomino::pnpsolve)